#include <Imx.h>
#include <net/if.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include "global.h"
#include "util.h"
#include "can.h"
#include "crc.h"


using namespace std;
using namespace Imx;

void print_frame(struct can_frame * frame);

Can::Can()
{
	_head = false;
	_tail = false;
	_cSock = 0;
	_listen = true;

	_dFrame.can_id &= CAN_EFF_MASK;
	_dFrame.can_id |= CAN_EFF_FLAG;
	_dFrame.can_dlc = 0;
}

Can::~Can()
{
	close(_cSock);
}

void
Can::init()
{
	struct ifreq ifr;

	_cSock = socket(PF_CAN, SOCK_RAW, CAN_RAW);
	strcpy(ifr.ifr_name, "can0");
	ioctl(_cSock, SIOCGIFINDEX, &ifr);

	_cAddr.can_family = AF_CAN;
	_cAddr.can_ifindex = ifr.ifr_ifindex;

	bind(_cSock, (struct sockaddr*)&_cAddr, sizeof(_cAddr));
}


// 数据接收
void
Can::listen()
{
	u32 nbytes, len;
	s32 ret;
	struct can_frame frame;

	while(_listen)
	{
		nbytes = recvfrom(_cSock, &frame, sizeof(struct can_frame), 0, (struct sockaddr*)&_cAddr, &len);
		pushFrame(frame);
	}
}

void
Can::detectNotify()
{
	_dFrame.can_id = CAN_EFF_FLAG | MCMD_HB | 0x00;
	_dFrame.can_dlc = 0;			// socketcan会随机填充数据:
	write(_cSock, &_dFrame, sizeof(_dFrame));
}

void
Can::dataNotify()  //广播采集
{
	//if(!_externalSource)
	{
		_dFrame.can_id = CAN_EFF_FLAG | MCMD_DATA | 0x00;
		_dFrame.can_dlc = 0;
		write(_cSock, &_dFrame, sizeof(_dFrame));
	}
}



// 配置命令下发
void
Can::stepNotify(u8 devId, u8 chnNo, Step ss)
{
	u8	offset;
	u16 crc16;
	u32	dev_id, dev_chn, data_len; 
	StepNotify sn;

	dev_id	= devId & PKT_ID_MASK;
	dev_chn	= (chnNo << 8) & PKT_CHN_MASK;
	data_len = ((sizeof(sn) + 2) << 11) & PKT_LEN_MASK; //with crc
	_dFrame.can_id = CAN_EFF_FLAG | MCMD_STEP | data_len | dev_chn | dev_id;
	printf("..................................[%x]\n",_dFrame.can_id);
	_dFrame.can_dlc = 8;
	
	sn.workMode = ss.workMode;
	memcpy(&sn.seq, &ss.seq, 16);
	
	crc16 = getCrc16((u8*)&sn, sizeof(sn));

	_dFrame.can_id &= ~PKT_TYPE_MASK;
	_dFrame.can_id |= TYPE_HEAD;
	offset = 0;
	memcpy(_dFrame.data, &sn.workMode, 8);

	offset +=8;
	write(_cSock, &_dFrame, sizeof(_dFrame));
	print_frame(&_dFrame);

	_dFrame.can_id &= ~PKT_TYPE_MASK;
	_dFrame.can_id |= TYPE_BODY;
	memcpy(_dFrame.data, &sn.workMode + offset, 8);
	offset +=8;
	write(_cSock, &_dFrame, sizeof(_dFrame));
	print_frame(&_dFrame);
/*
	_dFrame.can_id &= ~PKT_TYPE_MASK;
	_dFrame.can_id |= TYPE_BODY;
	memcpy(_dFrame.data, &sn.workMode + offset, 8);
	offset +=8;
	write(_cSock, &_dFrame, sizeof(_dFrame));
	print_frame(&_dFrame);
*/

	_dFrame.can_id &= ~PKT_TYPE_MASK;
	_dFrame.can_id |= TYPE_TAIL;
	memcpy(_dFrame.data, &sn.workMode + offset, 1);
	memcpy(&_dFrame.data[1], &crc16, 2);
	write(_cSock, &_dFrame, sizeof(_dFrame));
	print_frame(&_dFrame);
	printf("crc16 [0x%x]\n",crc16);
}

// 等待配置命令应答事件 需要和setNotify()配合调用
// id:连续启动多个工步后
s32
Can::setWaitAck(u32 *id, s32 timeout)
{
	IceUtil::Monitor<IceUtil::Mutex>::Lock lock(*this);
	struct can_frame frame;
	int ret;

	if(_setAckQueue.empty())
	{
		IceUtil::Time time = IceUtil::Time::seconds(timeout);
		timedWait(time);
	}

	// 超时处理
	if(_setAckQueue.empty())
		return IE_ACK_TIMEOUT;

	// 正常处理
	frame = _setAckQueue.front();
	_setAckQueue.pop();
	*id = frame.can_id;
	memcpy(&ret, &frame.data, 4);
	printf(">>>... SET ACK EID [0x%08x]  [%d]",frame.can_id, ret);
	return ret;
}

// 配置命令应答事件触发函数
void
Can::setPutAck(struct can_frame frame)
{
	IceUtil::Monitor<IceUtil::Mutex>::Lock lock(*this);
	_setAckQueue.push(frame);
	if(_setAckQueue.size() == 1)
	{
		notify();
	}
}

void
Can::setNotify(TaskProtect tp)
{
	u8	msg[8],offset;
	u32	devId,chnNo,dataLen;
	u16 crc16;

	devId = tp.dest & PKT_ID_MASK;
	chnNo = tp.dest & PKT_CHN_MASK;
	dataLen = ((sizeof(tp)+2)<<11) & PKT_LEN_MASK;
	_dFrame.can_id = CAN_EFF_FLAG | MCMD_SET | dataLen | chnNo | devId;
	printf("..................................[%x]\n",_dFrame.can_id);
	_dFrame.can_dlc = 8;

	crc16 = getCrc16((u8*)&tp,sizeof(tp));

	_dFrame.can_id &= ~PKT_TYPE_MASK;
	_dFrame.can_id |= TYPE_HEAD;
	memcpy(_dFrame.data, &tp.dest, 8);
	write(_cSock, &_dFrame, sizeof(_dFrame));

	_dFrame.can_id &= ~PKT_TYPE_MASK;
	_dFrame.can_id |= TYPE_BODY;
	memcpy(_dFrame.data, &tp.tempRange, 8);
	write(_cSock, &_dFrame, sizeof(_dFrame));

	_dFrame.can_id &= ~PKT_TYPE_MASK;
	_dFrame.can_id |= TYPE_BODY;
	memcpy(_dFrame.data, &tp.minVoltage, 8);
	write(_cSock, &_dFrame, sizeof(_dFrame));

	_dFrame.can_id &= ~PKT_TYPE_MASK;
	_dFrame.can_id |= TYPE_TAIL;
	memcpy(_dFrame.data, &tp.minCurrent, 4);
	memcpy((u8 *)&_dFrame.data+4, &crc16, 2);
	write(_cSock, &_dFrame, sizeof(_dFrame));

}

void
Can::finishNotify(u8 devId, u8 chnNo)
{
	u32	dev_id, dev_chn; 

	dev_id	= devId & PKT_ID_MASK;
	dev_chn	= (chnNo << 8) & PKT_CHN_MASK;
	_dFrame.can_id = CAN_EFF_FLAG | MCMD_FIN | dev_chn | dev_id;

	printf("can id fin [%08x]..........\n",_dFrame.can_id);
	write(_cSock, &_dFrame, sizeof(_dFrame));
}

void
Can::ack(u8 devId, u8 cnt)
{
}

struct can_frame
Can::popFrame()
{
	IceUtil::Monitor<IceUtil::Mutex>::Lock lock(*this);
	struct can_frame frame;

	while(_recvQueue.size() == 0)
	{
		wait();
	}
	frame = _recvQueue.front();
	_recvQueue.pop();
	return frame;
}

void
Can::pushFrame(struct can_frame frame)
{
	IceUtil::Monitor<IceUtil::Mutex>::Lock lock(*this);
	_recvQueue.push(frame);
	if(_recvQueue.size() == 1)
	{
		notify();
	}
}
/*
void
Can::setExternalSource(bool flag)
{
	_externalSource = true;
}

bool
Can::isExternalSource()
{
	return _externalSource;
}
*/

//======消息监听线程=========================
Listen::Listen():_stopped(false)
{
}

void
Listen::run()
{
	can0.listen();
}
